#include "CRF.h"
#include <cmath>

#define AMP_CONSTANT 1000

static Features* f = NULL;

int SmoothFn(int s1, int s2, int l1, int l2)
{
	/*int bd_length = f->spADM()->at<int>(s1,s2);
	float diff = 0; 
	if(l1 != l2){
		for(int c = 0; c < f->ChannelsNum(); c++){
			for(int d = 0; d < f->ChannelDimNum(c); d++){
				diff += (f->Channels(c)->at<float>(s1,d) - f->Channels(c)->at<float>(s2,d))
					*((f->Channels(c)->at<float>(s1,d) - f->Channels(c)->at<float>(s2,d)));
			}
		}
	}
	return (int)(-AMP_CONSTANT*(float)bd_length/(1.0f+sqrt(diff)));*/
	return 0;
}

void OptLabel(Features& ft, float* posprob, int* label)
{
	if(posprob == NULL || label == NULL)
		return;
	
	f = &ft;	
	int num_labels = 2;
	int num_sites = ft.SuperpixelNum();

	int* costs = new int[num_sites*num_labels];
	for(int i = 0; i < num_sites; i++){
		costs[i*num_labels]   = (int)(-AMP_CONSTANT*log(1-posprob[i]));        // cost for label 0
		costs[i*num_labels+1] = (int)(-AMP_CONSTANT*log(posprob[i]));          // cost for label 1
	}

	try{
		GCoptimizationGeneralGraph *gc = new GCoptimizationGeneralGraph(num_sites, num_labels);
		gc->setDataCost(costs);
		gc->setSmoothCost(&SmoothFn);

		for(int s1 = 0; s1 < num_sites; s1++){
			for(int s2 = s1; s2 < num_sites; s2++){
				if(ft.spADM()->at<int>(s1,s2) > 0)
					gc->setNeighbors(s1, s2, ft.spADM()->at<int>(s1,s2));
			}
		}

		printf("\nBefore optimization energy is %d",gc->compute_energy());
		gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
		printf("\nAfter optimization energy is %d",gc->compute_energy());

		for (int i = 0; i < num_sites; i++ )
			label[i] = gc->whatLabel(i);
		delete gc;
	}catch (GCException e){
		e.Report();
	}
}